from car_hardware import Motor
from measure import Alttitude
from pid import PID

class Car_Control():

    def __init__(self):

        #----------Hardware--------------------------
        self.motor_left = Motor(4, 0, ("/dev/gpiochip0", 29, "out"), ("/dev/gpiochip4", 11, "out"))
        self.motor_right = Motor(0, 0, ("/dev/gpiochip1", 18, "out"), ("/dev/gpiochip1", 20, "out"))

        self.motor_left.frequency = 500
        self.motor_right.frequency = 500

        self.motor_left.polarity = "normal"
        self.motor_right.polarity = "normal"

        self.motor_left.duty_cycle = 0
        self.motor_right.duty_cycle = 0

        self.motor_left.enable()
        self.motor_right.enable()

        #----------Execute Parameter----------------
        self.throttle = 0
        self.steer = 0

        #----------Measure Parameter----------------
        self.alttitude = Alttitude()

        #----------Control Parameter--------------
        self.steer_pid = PID(0.05, 0)
        self.speed_pid = PID(1, 0)
        self.max_steer = 0.5
        self.target_yaw = 0
        self.target_speed = 0

    def lock(self):
        self.motor_left.duty_cycle = 0
        self.motor_right.duty_cycle = 0
        self.motor_left.disable()
        self.motor_right.disable()


    def control_main_task(self):

        self.alttitude.Alttitude_Measure_Task()

        # -1 < throttle < 1
        self.speed_ctrl()
        self.steer_ctrl()
        if self.steer > self.max_steer:
            self.steer = self.max_steer
        elif self.steer < -self.max_steer:
            self.steer = -self.max_steer

        self.motor_left.set_throttle(self.throttle + self.steer, 0.45)
        self.motor_right.set_throttle(self.throttle - self.steer, 0.5)




    def steer_ctrl(self):
        if (self.throttle == 0):
            return
        
        current_yaw = self.alttitude.yaw
        #print(current_yaw - self.target_yaw, end = None, flush = False)
        self.steer_pid.update(current_yaw - self.target_yaw)

        self.steer = self.steer_pid.out


    def speed_ctrl(self):
        pass

    def car_start(self, thr):
        if not self.throttle == 0:
            return
        else:
            self.throttle = thr

if __name__ == "__main__":
    import time, os

    try:
        os.system("sh get_privilege.sh")

        mycar = Car_Control()

        # 油门
        mycar.car_start(0.8)

        count = 0
        while True:
            mycar.control_main_task()
            if count > 600:

                #---角度控制----
                mycar.target_yaw += 90
                #print(mycar.target_yaw)
                count = 0
            
            count += 1
    finally:
        mycar.lock()

